#pragma once
#include <d3d9.h>
#include <d3dx9math.h>
#include <math.h>
#include <memory.h>
#include "vector.h"
#include "boundingbox.h"

/**
 * Klasa macierzy 4x4
 *
 * @author Krzysztof Klein
 * @version 0.1
 */
class Matrix
{

	/**
	 * Tablica liczb w macierzy.
	 */
	float m[16];

public:

	/**
	 * Konstruktor domyslny.
	 * Laduje macierz tozsamosciowa
	 */
	inline Matrix() { *this = identity(); }

	/**
	 * Konstruktor z tablisy liczb.
	 * 
	 * @param t Tablica liczb
	 */
	inline Matrix(const float* t) { memcpy(m, t, sizeof(float) * 16); }

//#ifdef D3DMATRIX_DEFINED
	/**
	 * Konstruktor z macierzy Direct3D
	 */
	inline Matrix(const D3DMATRIX* mtx) { memcpy(m, mtx, sizeof(float) * 16); }
//#endif


	/**
	 * Zwraca macierz jako tablice liczb
	 *
	 * @return Tablica liczb
	 */
	inline operator const float*() const { return m; }
	inline operator float*() { return m; }


//#ifdef D3DMATRIX_DEFINED
	/**
	 * Zwraca macierz jako macierz Direct3D
	 */
	inline operator const D3DMATRIX*() const { return static_cast<const D3DMATRIX*>(static_cast<const void*>(m));}
	inline operator const D3DXMATRIX*() const { return static_cast<const D3DXMATRIX*>(static_cast<const void*>(m));}
//#endif


	/**
	 * Dodaje 2 macierze
	 */
	inline Matrix operator+(const Matrix &mtx) const
	{
		Matrix tmp;
#define o(i) tmp.m[i] = m[i] + mtx.m[i]
		 o(0);  o(1);  o(2);  o(3);
		 o(4);  o(5);  o(6);  o(7);
		 o(8);  o(9); o(10); o(11);
		o(12); o(13); o(14); o(15);
#undef o
		return tmp;
	}

	/**
	 * Dodaje macierz
	 */
	inline Matrix& operator+=(const Matrix &mtx)
	{
#define o(i) m[i] += mtx.m[i]
		 o(0);  o(1);  o(2);  o(3);
		 o(4);  o(5);  o(6);  o(7);
		 o(8);  o(9); o(10); o(11);
		o(12); o(13); o(14); o(15);
#undef o
		return *this;
	}

	/**
	 * Odejmuje 2 macierze
	 */
	inline Matrix operator-(const Matrix &mtx) const
	{
		Matrix tmp;
#define o(i) tmp.m[i] = m[i] - mtx.m[i]
		 o(0);  o(1);  o(2);  o(3);
		 o(4);  o(5);  o(6);  o(7);
		 o(8);  o(9); o(10); o(11);
		o(12); o(13); o(14); o(15);
#undef o
		return tmp;
	}

	/**
	 * Odejmuje macierz
	 */
	inline Matrix& operator-=(const Matrix &mtx)
	{
#define o(i) m[i] -= mtx.m[i]
		 o(0);  o(1);  o(2);  o(3);
		 o(4);  o(5);  o(6);  o(7);
		 o(8);  o(9); o(10); o(11);
		o(12); o(13); o(14); o(15);
#undef o
		return *this;
	}

	/**
	 * Mnozy 2 macierze
	 */
	inline Matrix operator*(const Matrix &mtx) const
	{
		Matrix tmp;
#define M tmp.m
#define m1 m
#define m2 mtx.m

		M[0] = m1[0]*m2[0] + m1[4]*m2[1] + m1[8]*m2[2] + m1[12]*m2[3];
		M[1] = m1[1]*m2[0] + m1[5]*m2[1] + m1[9]*m2[2] + m1[13]*m2[3];
		M[2] = m1[2]*m2[0] + m1[6]*m2[1] + m1[10]*m2[2] + m1[14]*m2[3];
		M[3] = m1[3]*m2[0] + m1[7]*m2[1] + m1[11]*m2[2] + m1[15]*m2[3];

		M[4] = m1[0]*m2[4] + m1[4]*m2[5] + m1[8]*m2[6] + m1[12]*m2[7];
		M[5] = m1[1]*m2[4] + m1[5]*m2[5] + m1[9]*m2[6] + m1[13]*m2[7];
		M[6] = m1[2]*m2[4] + m1[6]*m2[5] + m1[10]*m2[6] + m1[14]*m2[7];
		M[7] = m1[3]*m2[4] + m1[7]*m2[5] + m1[11]*m2[6] + m1[15]*m2[7];

		M[8] = m1[0]*m2[8] + m1[4]*m2[9] + m1[8]*m2[10] + m1[12]*m2[11];
		M[9] = m1[1]*m2[8] + m1[5]*m2[9] + m1[9]*m2[10] + m1[13]*m2[11];
		M[10] = m1[2]*m2[8] + m1[6]*m2[9] + m1[10]*m2[10] + m1[14]*m2[11];
		M[11] = m1[3]*m2[8] + m1[7]*m2[9] + m1[11]*m2[10] + m1[15]*m2[11];

		M[12] = m1[0]*m2[12] + m1[4]*m2[13] + m1[8]*m2[14] + m1[12]*m2[15];
		M[13] = m1[1]*m2[12] + m1[5]*m2[13] + m1[9]*m2[14] + m1[13]*m2[15];
		M[14] = m1[2]*m2[12] + m1[6]*m2[13] + m1[10]*m2[14] + m1[14]*m2[15];
		M[15] = m1[3]*m2[12] + m1[7]*m2[13] + m1[11]*m2[14] + m1[15]*m2[15];

		return tmp;
	}

	/**
	 * Mnozy przez macierz
	 */
	inline Matrix& operator*=(const Matrix &mtx)
	{
		return *this = *this * mtx;
	}

	/**
	 * Mnozy wektor przez macierz
	 */
	inline Vector operator*(const Vector &v) const
	{
		return Vector(
			v.getX()*m[0] + v.getY()*m[4] + v.getZ()*m[8]  + m[12],
			v.getX()*m[1] + v.getY()*m[5] + v.getZ()*m[9]  + m[13],
			v.getX()*m[2] + v.getY()*m[6] + v.getZ()*m[10] + m[14]);
	}

	/**
	 * Mnozy przez skalar
	 */
	inline Matrix operator*(float t) const
	{
		Matrix tmp;
#define o(i) tmp.m[i] = m[i] * t
		 o(0);  o(1);  o(2);  o(3);
		 o(4);  o(5);  o(6);  o(7);
		 o(8);  o(9); o(10); o(11);
		o(12); o(13); o(14); o(15);
#undef o
		return tmp;
	}

	/**
	 * Mnozy przez skalar
	 */
	inline Matrix& operator*=(float t)
	{
#define o(i) m[i] *= t
		 o(0);  o(1);  o(2);  o(3);
		 o(4);  o(5);  o(6);  o(7);
		 o(8);  o(9); o(10); o(11);
		o(12); o(13); o(14); o(15);
#undef o
		return *this;
	}


	/**
	 * Zwraca macierz tozsamosciowa
	 */
	inline static Matrix identity()
	{
		static const float identityMatrix[16] = 
		{
			1.0f, 0.0f, 0.0f, 0.0f,
			0.0f, 1.0f, 0.0f, 0.0f,
			0.0f, 0.0f, 1.0f, 0.0f,
			0.0f, 0.0f, 0.0f, 1.0f
		};
		return Matrix(identityMatrix);
	}


	/**
	 * Zwraca wektor przesuniecia macierzy transformacji
	 */
	inline Vector getTranslation() const
	{
		return Vector(m[12], m[13], m[14]);
	}

	/**
	 * Ustawia przesuniecie na zadany wektor.
	 *
	 * @param v Wektor przesuniecia
	 */
	inline Matrix& setTranslation(const Vector &v)
	{
		m[12] = v.getX();
		m[13] = v.getY();
		m[14] = v.getZ();
		return *this;
	}


	/**
	 * Zwraca wektor obrotu macierzy transformacji
	 */
	inline Vector getRotation() const
	{
		// http://www.robertblum.com/articles/2005/02/14/decomposing-matrices

		Vector scale = getScale();
		Vector invScale = Vector(1,1,1) / scale;

		float x, y, z, c;
		y = -asin(m[2] * invScale.getX());
		c = cos(y);

		if(!(fabs(c) < 0.0001f))
		{
			float invC = 1.0f / c;

			x = atan2(
				m[6] * invC * invScale.getY(),
				m[10] * invC * invScale.getZ());
			z = atan2(
				m[1] * invC * invScale.getX(),
				m[0] * invC * invScale.getX());
		}
		else
		{
			x = 0.0f;
			z = atan2(
				-m[4] * invScale.getY(),
				m[5] * invScale.getY());
		}
		return Vector(x, y, z);
	}

	/**
	 * Ustawia obrot na zadany wektor.
	 *
	 * @param v Wektor obrotu (katy Eulera w radianach)
	 */
	inline Matrix& setRotation(const Vector &v)
	{
		const float cr = cos( v.getX() );
		const float sr = sin( v.getX() );
		const float cp = cos( v.getY() );
		const float sp = sin( v.getY() );
		const float cy = cos( v.getZ() );
		const float sy = sin( v.getZ() );
		const float srsp = sr * sp;
		const float crsp = cr * sp;

		m[0] = cp * cy;
		m[1] = cp * sy;
		m[2] = -sp;

		m[4] = srsp * cy - cr * sy;
		m[5] = srsp * sy + cr * cy;
		m[6] = sr * cp;

		m[8] = crsp * cy + sr * sy;
		m[9] = crsp * sy - sr * cy;
		m[10] = cr * cp;

		return *this;
	}


	/**
	 * Zwraca wektor skali macierzy transformacji
	 */
	inline Vector getScale() const
	{
		
		return Vector(
			sqrt(m[0]*m[0] + m[1]*m[1] + m[2]*m[2]),
			sqrt(m[4]*m[4] + m[5]*m[5] + m[6]*m[6]),
			sqrt(m[8]*m[8] + m[9]*m[9] + m[10]*m[10]));
			
		/*return Vector(m[0], m[5], m[10]);*/
	}

	/**
	 * Ustawia skale na zadany wektor.
	 *
	 * @param v Wektor skali
	 */
	inline Matrix& setScale(const Vector &v)
	{
		m[0] = v.getX();
		m[5] = v.getY();
		m[10] = v.getZ();
		return *this;
	}


	/**
	 * Zwraca macierz projekcji perspektywicznej (lewoskretnej).
	 *
	 * @param fov Kat patrzenia (w radianach)
	 * @param aspect Stosunek wysokosci do szerokosci
	 * @param zNear
	 * @param zFar
	 * @return Macierz projekcji
	 */
	inline static Matrix projectionPerspective(
			float fov, float aspect, float zNear, float zFar)
	{
		Matrix mtx;
		if(fabs(fov) < 0.001f) return mtx;
		if(fabs(aspect) < 0.001f) return mtx;
		if(fabs(zFar - zNear) < 0.001f) return mtx;

		const float h = 1.0f / (tan(fov * 0.5f));
		const float w = (h / aspect);

		mtx.m[0] = w;
		mtx.m[1] = 0.0f;
		mtx.m[2] = 0.0f;
		mtx.m[3] = 0.0f;

		mtx.m[4] = 0.0f;
		mtx.m[5] = h;
		mtx.m[6] = 0.0f;
		mtx.m[7] = 0.0f;

		mtx.m[8] = 0.0f;
		mtx.m[9] = 0.0f;
		mtx.m[10] = zFar / (zFar - zNear);
		mtx.m[11] = 1.0f;

		mtx.m[12] = 0.0f;
		mtx.m[13] = 0.0f;
		mtx.m[14] = (-zNear * zFar) / (zFar - zNear);
		mtx.m[15] = 0.0f;

		return mtx;
	}


	/**
	 * Zwraca macierz projekcji ortogonalnej (lewoskretnej).
	 *
	 * @param width Szerokosc
	 * @param height Wysokosc
	 * @param zNear
	 * @param zFar
	 * @return Macierz projekcji
	 */
	inline static Matrix projectionOrtho(
			float width, float height, float zNear, float zFar)
	{
		Matrix mtx;
		if(fabs(width) < 0.001f) return mtx;
		if(fabs(height) < 0.001f) return mtx;
		if(fabs(zFar - zNear) < 0.001f) return mtx;

		mtx.m[0] = 2.0f / width;
		mtx.m[1] = 0.0f;
		mtx.m[2] = 0.0f;
		mtx.m[3] = 0.0f;

		mtx.m[4] = 0.0f;
		mtx.m[5] = 2.0f / height;
		mtx.m[6] = 0.0f;
		mtx.m[7] = 0.0f;

		mtx.m[8] = 0.0f;
		mtx.m[9] = 0.0f;
		mtx.m[10] = 1.0f / (zFar - zNear);
		mtx.m[11] = 0.0f;

		mtx.m[12] = 0.0f;
		mtx.m[13] = 0.0f;
		mtx.m[14] = zNear / (zNear - zFar);
		mtx.m[15] = 1.0f;

		return mtx;
	}

	/**
	 * Zwraca macierz transformacji kamery.
	 *
	 * @param position Pozycja kamery
	 * @param target Cel kamery
	 * @param up Wektor gory
	 * @return Macierz transformacji kamery
	 */
	inline static Matrix cameraLookAt(
			const Vector &position,
			const Vector &target,
			const Vector &up)
	{
		Matrix mtx;

		Vector zaxis = target - position;
		zaxis.normalize();

		Vector xaxis = up.crossProduct(zaxis);
		xaxis.normalize();

		Vector yaxis = zaxis.crossProduct(xaxis);

		mtx.m[0] = xaxis.getX();
		mtx.m[1] = yaxis.getX();
		mtx.m[2] = zaxis.getX();
		mtx.m[3] = 0.0f;

		mtx.m[4] = xaxis.getY();
		mtx.m[5] = yaxis.getY();
		mtx.m[6] = zaxis.getY();
		mtx.m[7] = 0.0f;

		mtx.m[8] = xaxis.getZ();
		mtx.m[9] = yaxis.getZ();
		mtx.m[10] = zaxis.getZ();
		mtx.m[11] = 0.0f;

		mtx.m[12] = -xaxis.dotProduct(position);
		mtx.m[13] = -yaxis.dotProduct(position);
		mtx.m[14] = -zaxis.dotProduct(position);
		mtx.m[15] = 1.0f;

		return mtx;
	}

	/**
	 * Interpoluje macierz
	 *
	 * @param m Druga macierz
	 * @param t Wspolczynnik interpolacji
	 * @return Zinterpolowana macierz
	 */
	inline Matrix interpolate(const Matrix &mtx, float t) const { return (*this)*t + mtx*(1.0f-t); }


	/**
	 * Przeksztalca prostopadloscian otaczajacy
	 */
	inline BoundingBox operator*(BoundingBox box) const
	{
		const Vector aMin = box.getMinPoint();
		const Vector aMax = box.getMaxPoint();

		Vector bMin;
		Vector bMax;

		bMin[0] = bMax[0] = m[12];
		bMin[1] = bMax[1] = m[13];
		bMin[2] = bMax[2] = m[14];

		for(int i = 0; i < 3; i++)
		{
			for(int j = 0; j < 3; j++)
			{
				const float a = m[4*j + i] * aMin[j];
				const float b = m[4*j + i] * aMax[j];

				if (a < b)
				{
					bMin[i] += a;
					bMax[i] += b;
				}
				else
				{
					bMin[i] += b;
					bMax[i] += a;
				}
			}
		}

		box.getMinPoint() = bMin;
		box.getMaxPoint() = bMax;

		return box;
	}


	/**
	 * Oblicza macierz odwrotna
	 */
	Matrix getInverse() const
	{
		Matrix inverse;

#define mtx(i, j) (m[4*(i)+(j)])
#define out(i, j) (inverse.m[4*(i)+(j)])

		// oblicz wyznacznik
		float det = (mtx(0, 0) * mtx(1, 1) - mtx(0, 1) * mtx(1, 0)) * (mtx(2, 2) * mtx(3, 3) - mtx(2, 3) * mtx(3, 2)) -
			(mtx(0, 0) * mtx(1, 2) - mtx(0, 2) * mtx(1, 0)) * (mtx(2, 1) * mtx(3, 3) - mtx(2, 3) * mtx(3, 1)) +
			(mtx(0, 0) * mtx(1, 3) - mtx(0, 3) * mtx(1, 0)) * (mtx(2, 1) * mtx(3, 2) - mtx(2, 2) * mtx(3, 1)) +
			(mtx(0, 1) * mtx(1, 2) - mtx(0, 2) * mtx(1, 1)) * (mtx(2, 0) * mtx(3, 3) - mtx(2, 3) * mtx(3, 0)) -
			(mtx(0, 1) * mtx(1, 3) - mtx(0, 3) * mtx(1, 1)) * (mtx(2, 0) * mtx(3, 2) - mtx(2, 2) * mtx(3, 0)) +
			(mtx(0, 2) * mtx(1, 3) - mtx(0, 3) * mtx(1, 2)) * (mtx(2, 0) * mtx(3, 1) - mtx(2, 1) * mtx(3, 0));

		if(fabs(det) < 0.00001f)
			return identity();

		det = 1.0f / det;

		out(0, 0) = det * (mtx(1, 1) * (mtx(2, 2) * mtx(3, 3) - mtx(2, 3) * mtx(3, 2)) +
				mtx(1, 2) * (mtx(2, 3) * mtx(3, 1) - mtx(2, 1) * mtx(3, 3)) +
				mtx(1, 3) * (mtx(2, 1) * mtx(3, 2) - mtx(2, 2) * mtx(3, 1)));
		out(0, 1) = det * (mtx(2, 1) * (mtx(0, 2) * mtx(3, 3) - mtx(0, 3) * mtx(3, 2)) +
				mtx(2, 2) * (mtx(0, 3) * mtx(3, 1) - mtx(0, 1) * mtx(3, 3)) +
				mtx(2, 3) * (mtx(0, 1) * mtx(3, 2) - mtx(0, 2) * mtx(3, 1)));
		out(0, 2) = det * (mtx(3, 1) * (mtx(0, 2) * mtx(1, 3) - mtx(0, 3) * mtx(1, 2)) +
				mtx(3, 2) * (mtx(0, 3) * mtx(1, 1) - mtx(0, 1) * mtx(1, 3)) +
				mtx(3, 3) * (mtx(0, 1) * mtx(1, 2) - mtx(0, 2) * mtx(1, 1)));
		out(0, 3) = det * (mtx(0, 1) * (mtx(1, 3) * mtx(2, 2) - mtx(1, 2) * mtx(2, 3)) +
				mtx(0, 2) * (mtx(1, 1) * mtx(2, 3) - mtx(1, 3) * mtx(2, 1)) +
				mtx(0, 3) * (mtx(1, 2) * mtx(2, 1) - mtx(1, 1) * mtx(2, 2)));
		out(1, 0) = det * (mtx(1, 2) * (mtx(2, 0) * mtx(3, 3) - mtx(2, 3) * mtx(3, 0)) +
				mtx(1, 3) * (mtx(2, 2) * mtx(3, 0) - mtx(2, 0) * mtx(3, 2)) +
				mtx(1, 0) * (mtx(2, 3) * mtx(3, 2) - mtx(2, 2) * mtx(3, 3)));
		out(1, 1) = det * (mtx(2, 2) * (mtx(0, 0) * mtx(3, 3) - mtx(0, 3) * mtx(3, 0)) +
				mtx(2, 3) * (mtx(0, 2) * mtx(3, 0) - mtx(0, 0) * mtx(3, 2)) +
				mtx(2, 0) * (mtx(0, 3) * mtx(3, 2) - mtx(0, 2) * mtx(3, 3)));
		out(1, 2) = det * (mtx(3, 2) * (mtx(0, 0) * mtx(1, 3) - mtx(0, 3) * mtx(1, 0)) +
				mtx(3, 3) * (mtx(0, 2) * mtx(1, 0) - mtx(0, 0) * mtx(1, 2)) +
				mtx(3, 0) * (mtx(0, 3) * mtx(1, 2) - mtx(0, 2) * mtx(1, 3)));
		out(1, 3) = det * (mtx(0, 2) * (mtx(1, 3) * mtx(2, 0) - mtx(1, 0) * mtx(2, 3)) +
				mtx(0, 3) * (mtx(1, 0) * mtx(2, 2) - mtx(1, 2) * mtx(2, 0)) +
				mtx(0, 0) * (mtx(1, 2) * mtx(2, 3) - mtx(1, 3) * mtx(2, 2)));
		out(2, 0) = det * (mtx(1, 3) * (mtx(2, 0) * mtx(3, 1) - mtx(2, 1) * mtx(3, 0)) +
				mtx(1, 0) * (mtx(2, 1) * mtx(3, 3) - mtx(2, 3) * mtx(3, 1)) +
				mtx(1, 1) * (mtx(2, 3) * mtx(3, 0) - mtx(2, 0) * mtx(3, 3)));
		out(2, 1) = det * (mtx(2, 3) * (mtx(0, 0) * mtx(3, 1) - mtx(0, 1) * mtx(3, 0)) +
				mtx(2, 0) * (mtx(0, 1) * mtx(3, 3) - mtx(0, 3) * mtx(3, 1)) +
				mtx(2, 1) * (mtx(0, 3) * mtx(3, 0) - mtx(0, 0) * mtx(3, 3)));
		out(2, 2) = det * (mtx(3, 3) * (mtx(0, 0) * mtx(1, 1) - mtx(0, 1) * mtx(1, 0)) +
				mtx(3, 0) * (mtx(0, 1) * mtx(1, 3) - mtx(0, 3) * mtx(1, 1)) +
				mtx(3, 1) * (mtx(0, 3) * mtx(1, 0) - mtx(0, 0) * mtx(1, 3)));
		out(2, 3) = det * (mtx(0, 3) * (mtx(1, 1) * mtx(2, 0) - mtx(1, 0) * mtx(2, 1)) +
				mtx(0, 0) * (mtx(1, 3) * mtx(2, 1) - mtx(1, 1) * mtx(2, 3)) +
				mtx(0, 1) * (mtx(1, 0) * mtx(2, 3) - mtx(1, 3) * mtx(2, 0)));
		out(3, 0) = det * (mtx(1, 0) * (mtx(2, 2) * mtx(3, 1) - mtx(2, 1) * mtx(3, 2)) +
				mtx(1, 1) * (mtx(2, 0) * mtx(3, 2) - mtx(2, 2) * mtx(3, 0)) +
				mtx(1, 2) * (mtx(2, 1) * mtx(3, 0) - mtx(2, 0) * mtx(3, 1)));
		out(3, 1) = det * (mtx(2, 0) * (mtx(0, 2) * mtx(3, 1) - mtx(0, 1) * mtx(3, 2)) +
				mtx(2, 1) * (mtx(0, 0) * mtx(3, 2) - mtx(0, 2) * mtx(3, 0)) +
				mtx(2, 2) * (mtx(0, 1) * mtx(3, 0) - mtx(0, 0) * mtx(3, 1)));
		out(3, 2) = det * (mtx(3, 0) * (mtx(0, 2) * mtx(1, 1) - mtx(0, 1) * mtx(1, 2)) +
				mtx(3, 1) * (mtx(0, 0) * mtx(1, 2) - mtx(0, 2) * mtx(1, 0)) +
				mtx(3, 2) * (mtx(0, 1) * mtx(1, 0) - mtx(0, 0) * mtx(1, 1)));
		out(3, 3) = det * (mtx(0, 0) * (mtx(1, 1) * mtx(2, 2) - mtx(1, 2) * mtx(2, 1)) +
				mtx(0, 1) * (mtx(1, 2) * mtx(2, 0) - mtx(1, 0) * mtx(2, 2)) +
				mtx(0, 2) * (mtx(1, 0) * mtx(2, 1) - mtx(1, 1) * mtx(2, 0)));

		return inverse;

#undef mtx
#undef out

	}

};
